﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Timers;

namespace DTW_Monitor
{
    public partial class MainForm : Form
    {
        private JoystickHandler joy = new JoystickHandler();

        public enum MessageType { Incoming, Outgoing, Normal, Warning, Error };

        NmeaParserDllInterface nmeaPI = new NmeaParserDllInterface();
        private Color[] MessageColor = { Color.Blue, Color.Green, Color.Black, Color.Orange, Color.Red };
        private SerialPort comPort = new SerialPort();
        private bool sendingRecieving = false;

        decimal leftMotorOffset = 0;
        decimal rightMotorOffset = 0;
        decimal rearMotorOffset = 0;
        decimal rearServoOffset = 0;

        System.Timers.Timer aTimer = new System.Timers.Timer();

        private static void OnTimedEvent()
        {
            Console.WriteLine("Hello World!");
        }

        public MainForm()
        {
            InitializeComponent();

            comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived);

            joy.Init(this.Handle);

          
            // Left Motor
            numericUpDownLeftMotor.Minimum = 1000;
            numericUpDownLeftMotor.Maximum = 2000;
            numericUpDownLeftMotor.Value = numericUpDownLeftMotor.Minimum;
            trackBarLeftMotor.Minimum = Convert.ToInt32(numericUpDownLeftMotor.Minimum);
            trackBarLeftMotor.Maximum = Convert.ToInt32(numericUpDownLeftMotor.Maximum);
            numericUpDownLeftMotor.DataBindings.Add("Value", trackBarLeftMotor, "Value", false,  DataSourceUpdateMode.OnPropertyChanged);
            labelLeftMotorMin.Text = numericUpDownLeftMotor.Minimum.ToString();
            labelLeftMotorMax.Text = numericUpDownLeftMotor.Maximum.ToString();
            
            // Right Motor
            numericUpDownRightMotor.Minimum = 1000;
            numericUpDownRightMotor.Maximum = 2000;
            numericUpDownRightMotor.Value = numericUpDownRightMotor.Minimum;
            trackBarRightMotor.Minimum = Convert.ToInt32(numericUpDownRightMotor.Minimum);
            trackBarRightMotor.Maximum = Convert.ToInt32(numericUpDownRightMotor.Maximum);
            numericUpDownRightMotor.DataBindings.Add("Value", trackBarRightMotor, "Value", false, DataSourceUpdateMode.OnPropertyChanged);
            labelRightMotorMin.Text = numericUpDownRightMotor.Minimum.ToString();
            labelRightMotorMax.Text = numericUpDownRightMotor.Maximum.ToString();


            // Throttle
            numericUpDownThrottle.Minimum = 0;
            numericUpDownThrottle.Maximum = 1000;
            trackBarThrottle.Minimum = Convert.ToInt32(numericUpDownThrottle.Minimum);
            trackBarThrottle.Maximum = Convert.ToInt32(numericUpDownThrottle.Maximum);
            numericUpDownThrottle.DataBindings.Add("Value", trackBarThrottle, "Value", false, DataSourceUpdateMode.OnPropertyChanged);
            labelThrottleMin.Text = numericUpDownThrottle.Minimum.ToString();
            labelThrottleMax.Text = numericUpDownThrottle.Maximum.ToString();
                        
            // Rear Motor
            numericUpDownRearMotor.Minimum = 0;
            numericUpDownRearMotor.Maximum = 100;
            numericUpDownRearMotor.Value = numericUpDownRearMotor.Minimum;
            trackBarRearMotor.Minimum = Convert.ToInt32(numericUpDownRearMotor.Minimum);
            trackBarRearMotor.Maximum = Convert.ToInt32(numericUpDownRearMotor.Maximum);
            numericUpDownRearMotor.DataBindings.Add("Value", trackBarRearMotor, "Value", false, DataSourceUpdateMode.OnPropertyChanged);
            labelRearMotorMin.Text = numericUpDownRearMotor.Minimum.ToString();
            labelRearMotorMax.Text = numericUpDownRearMotor.Maximum.ToString();

            // Rear Servo
            numericUpDownRearServo.Minimum = -30;
            numericUpDownRearServo.Maximum = 30;
            numericUpDownRearServo.Value = 0;
            trackBarRearServo.Minimum = Convert.ToInt32(numericUpDownRearServo.Minimum);
            trackBarRearServo.Maximum = Convert.ToInt32(numericUpDownRearServo.Maximum);
            numericUpDownRearServo.DataBindings.Add("Value", trackBarRearServo, "Value", false, DataSourceUpdateMode.OnPropertyChanged);
            labelRearServoMin.Text = numericUpDownRearServo.Minimum.ToString();
            labelRearServoMax.Text = numericUpDownRearServo.Maximum.ToString();
         }

        private void MainForm_Load(object sender, EventArgs e)
        {
            // Set Parity Values
            foreach (string str in Enum.GetNames(typeof(Parity)))
                comboBoxParity.Items.Add(str);
            comboBoxParity.SelectedIndex = 0;
            //Set Stop Bit Values(object obj)
            foreach (string str in Enum.GetNames(typeof(StopBits)))
                comboBoxStopBits.Items.Add(str);
            comboBoxStopBits.SelectedIndex = 1;
            // Set Port Name Values        {
            foreach (string str in SerialPort.GetPortNames())
                comboBoxPort.Items.Add(str);
            comboBoxPort.SelectedIndex = 0;

            comboBoxBaudRate.SelectedText = "57600";
            comboBoxDataBits.SelectedIndex = 1;
        }

        #region ComPort I/O
       
        public bool OpenPort()
        {
            try
            {
                // Close port if is already open
                if (comPort.IsOpen == true)
                    ClosePort();

                // Set the properties of our SerialPort Object
                comPort.PortName = comboBoxPort.Text;
                comPort.BaudRate = int.Parse(comboBoxBaudRate.Text);
                comPort.Parity = (Parity)Enum.Parse(typeof(Parity), comboBoxParity.Text);
                comPort.StopBits = (StopBits)Enum.Parse(typeof(StopBits), comboBoxStopBits.Text);
                comPort.DataBits = int.Parse(comboBoxDataBits.Text);
                comPort.Open();

                DisplayData(MessageType.Normal, "Port opened at " + comboBoxPort.Text + " at " + DateTime.Now + "\n");

                return true;
            }
            catch (Exception ex)
            {
                DisplayData(MessageType.Error, ex.Message);
                return false;
            }
        }

        public bool ClosePort()
        {
            try
            {
                // Close port if is open
                if (comPort.IsOpen)
                {
                    comPort.Close();
                    DisplayData(MessageType.Normal, "Port at " + comboBoxPort.Text + " closed at " + DateTime.Now + "\n");
                }

                return true;
            }
            catch (Exception ex)
            {
                DisplayData(MessageType.Error, ex.Message);
                return false;
            }
        }

        public void WriteData(string str)
        {
            if (!comPort.IsOpen)
                comPort.Open();

            comPort.Write(str);

            DisplayData(MessageType.Outgoing, str + "\n");
        }

        public string ReadData()
        {
            string str = comPort.ReadExisting();
            
            DisplayData(MessageType.Incoming, str + "\n");

            return str;
        }

        #endregion //ComPort I/O

        #region Other
        /// <summary>
        /// Method that will be called when theres data waiting in the serial port buffer
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void comPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            string str = ReadData();

            if (sendingRecieving)
            {
                NmeaParserDllInterface.ParseDll(str);
                if (numericUpDownLeftMotor.InvokeRequired)
                {
                    ChangeLeftMotorValueDelegate lm = new ChangeLeftMotorValueDelegate(ChangeLeftMotorValue);
                    numericUpDownLeftMotor.Invoke(lm, new object[] {NmeaParserDllInterface.GetLeftMotorDll()});
                }
                else
                {
                    numericUpDownLeftMotor.Value = Convert.ToDecimal(NmeaParserDllInterface.GetLeftMotorDll());
                }
                numericUpDownLeftMotor.Value = Convert.ToDecimal(NmeaParserDllInterface.GetLeftMotorDll());
                numericUpDownRightMotor.Value =Convert.ToDecimal(NmeaParserDllInterface.GetRightMotorDll());
                numericUpDownRearMotor.Value = Convert.ToDecimal(NmeaParserDllInterface.GetRearMotorDll());
                numericUpDownRearServo.Value = Convert.ToDecimal(NmeaParserDllInterface.GetRearServoDll());
            }
        }

        public delegate void ChangeLeftMotorValueDelegate(int value);
        public void ChangeLeftMotorValue(int value)
        {
            numericUpDownLeftMotor.Value = Convert.ToDecimal(value);
        }

        /// <summary>
        /// Method to display the data to & from the port on the screen
        /// </summary>
        /// <param name="type">MessageType of the message</param>
        /// <param name="msg">Message to display</param>
        [STAThread]
        private void DisplayData(MessageType type, string msg)
        {
            richTextBoxDisplay.Invoke(new EventHandler(delegate
            {
                richTextBoxDisplay.SelectedText = string.Empty;
                richTextBoxDisplay.SelectionFont = new Font(richTextBoxDisplay.SelectionFont, FontStyle.Bold);
                richTextBoxDisplay.SelectionColor = MessageColor[(int)type];
                richTextBoxDisplay.AppendText(msg);
                richTextBoxDisplay.ScrollToCaret();
            }));
        }
        #endregion

        #region Buttons
        private void buttonOpenClose_Click(object sender, EventArgs e)
        {
            if (buttonOpenClose.Text == "Open Port")
            {
                OpenPort();
                buttonOpenClose.Text = "Close Port";
                
                buttonSend.Enabled = true;
                buttonSendRecieveData.Enabled = true;
                comboBoxPort.Enabled = false;
                comboBoxBaudRate.Enabled = false;
                comboBoxParity.Enabled = false;
                comboBoxDataBits.Enabled = false;
                comboBoxStopBits.Enabled = false;
                radioButtonRecieving.Enabled = false;
                radioButtonSending.Enabled = false;
            }
            else
            {
                ClosePort();
                buttonOpenClose.Text = "Open Port";
                
                buttonSend.Enabled = false;
                buttonSendRecieveData.Enabled = false;
                comboBoxPort.Enabled = true;
                comboBoxBaudRate.Enabled = true;
                comboBoxParity.Enabled = true;
                comboBoxDataBits.Enabled = true;
                comboBoxStopBits.Enabled = true;
                radioButtonRecieving.Enabled = true;
                radioButtonSending.Enabled = true;

                string recieveSend = radioButtonSending.Checked ? "Sending" : "Recieving";
                buttonSendRecieveData.Text = "Start " + recieveSend + " Data";
                sendingRecieving = false;
            }
        }           

        private void buttonSend_Click(object sender, EventArgs e)
        {
            WriteData(textBoxSend.Text);
        }
               
        private void buttonSendRecieveData_Click(object sender, EventArgs e)
        {
            sendingRecieving = !sendingRecieving;

            if (sendingRecieving)
            {
                if (radioButtonSending.Checked)
                {

                    aTimer = new System.Timers.Timer();
                    aTimer.Elapsed += new ElapsedEventHandler(SendMe);
                    // Set the Interval to 5 seconds.
                    aTimer.Interval = Convert.ToDouble(1000 * 1 / numericUpDownFrequency.Value);
                    aTimer.Enabled = true;
                }
            }
            else 
            {
                if (radioButtonSending.Checked)
                {
                    aTimer.Enabled = false;
                    aTimer = null;
                                       
                }
            }


            string startStop = sendingRecieving ? "Stop" : "Start";
            string recieveSend = radioButtonSending.Checked ? "Sending" : "Recieving";
            buttonSendRecieveData.Text = startStop + " " + recieveSend + " Data"; 
        }
        #endregion //Buttons

        private void SendMe(object source, ElapsedEventArgs e)
        {
            NmeaParserDllInterface.SetLeftMotorDll((int)numericUpDownLeftMotor.Value);
            NmeaParserDllInterface.SetRightMotorDll((int)numericUpDownRightMotor.Value);
            NmeaParserDllInterface.SetRearMotorDll((float)numericUpDownRearMotor.Value);
            NmeaParserDllInterface.SetRearServoDll((float)numericUpDownRearServo.Value);
            WriteData(NmeaParserDllInterface.GenerateDTWX3Dll());
        }

        private void radioButtonSending_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButtonSending.Checked)
            {
                buttonSendRecieveData.Text = "Send Data";

                trackBarLeftMotor.Enabled = true;
                trackBarRightMotor.Enabled = true;
                trackBarRearMotor.Enabled = true;
                trackBarRearServo.Enabled = true;
                numericUpDownLeftMotor.Enabled = true;
                numericUpDownRightMotor.Enabled = true;
                numericUpDownRearMotor.Enabled = true;
                numericUpDownRearServo.Enabled = true;
                numericUpDownFrequency.Enabled = true;
            }
            else
            {
                buttonSendRecieveData.Text = "Start recieveing Data";

                trackBarLeftMotor.Enabled = false;
                trackBarRightMotor.Enabled = false;
                trackBarRearMotor.Enabled = false;
                trackBarRearServo.Enabled = false;
                numericUpDownLeftMotor.Enabled = false;
                numericUpDownRightMotor.Enabled = false;
                numericUpDownRearMotor.Enabled = false;
                numericUpDownRearServo.Enabled = false;
                numericUpDownFrequency.Enabled = false;
            }
        }

        private void MainForm_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyData == Keys.Space)
            {
                Abort();
                return;
            }

            if (e.KeyData == Keys.W)
            {
                if (numericUpDownLeftMotor.Value < numericUpDownLeftMotor.Maximum)
                    numericUpDownLeftMotor.Value++;
            }
            else if (e.KeyData == Keys.S)
            {
                if (numericUpDownLeftMotor.Value > numericUpDownLeftMotor.Minimum)
                    numericUpDownLeftMotor.Value--;
            }
            
            if (e.KeyData == Keys.E)
            {
                if (numericUpDownRightMotor.Value < numericUpDownRightMotor.Maximum)
                    numericUpDownRightMotor.Value++;
            }
            else if (e.KeyData == Keys.D)
            {
                if (numericUpDownRightMotor.Value > numericUpDownRightMotor.Minimum)
                    numericUpDownRightMotor.Value--;
            }
            
            if (e.KeyData == Keys.I)
            {
                if (numericUpDownRearMotor.Value < numericUpDownRearMotor.Maximum)
                    numericUpDownRearMotor.Value++;
            }
            else if (e.KeyData == Keys.K)
            {
                if (numericUpDownRearMotor.Value > numericUpDownRearMotor.Minimum)
                    numericUpDownRearMotor.Value--;
            }
            
            if (e.KeyData == Keys.L)
            {
                if (numericUpDownRearServo.Value < numericUpDownRearServo.Maximum)
                    numericUpDownRearServo.Value++;
            }
            else if (e.KeyData == Keys.J)
            {
                if (numericUpDownRearServo.Value > numericUpDownRearServo.Minimum)
                    numericUpDownRearServo.Value--;
            }


        }

        private void timerJoystick_Tick(object sender, EventArgs e)
        {
            JoyState s= joy.Poll();

            // NEW: trimming
            rearServoOffset = (decimal)(s.Trim1) * 20;
            rearMotorOffset = (decimal)(s.Trim2+1)*30;
            
            //decimal throttle = numericUpDownThrottle.Value;
            decimal throttle = (decimal)(s.Throttle * 1000);

            decimal leftMotorValue = numericUpDownLeftMotor.Minimum + throttle + (decimal)(s.AxisX * 100) + leftMotorOffset;
            if (leftMotorValue < numericUpDownLeftMotor.Minimum)
                numericUpDownLeftMotor.Value = numericUpDownLeftMotor.Minimum;
            else if (leftMotorValue > numericUpDownLeftMotor.Maximum)
                numericUpDownLeftMotor.Value = numericUpDownLeftMotor.Maximum;
            else
                numericUpDownLeftMotor.Value = leftMotorValue;

            decimal rightMotorValue = numericUpDownRightMotor.Minimum + throttle - (decimal)(s.AxisX * 100) + rightMotorOffset;
            if (rightMotorValue < numericUpDownRightMotor.Minimum)
                numericUpDownRightMotor.Value = numericUpDownRightMotor.Minimum;
            else if (rightMotorValue > numericUpDownRightMotor.Maximum)
                numericUpDownRightMotor.Value = numericUpDownRightMotor.Maximum;
            else
                numericUpDownRightMotor.Value = rightMotorValue;

            decimal rearMotorValue = (decimal)(s.AxisY * 100) + rearMotorOffset;
            if (rearMotorValue < numericUpDownRearMotor.Minimum)
                numericUpDownRearMotor.Value = numericUpDownRearMotor.Minimum;
            else if (rearMotorValue > numericUpDownRearMotor.Maximum)
                numericUpDownRearMotor.Value = numericUpDownRearMotor.Maximum;
            else
                numericUpDownRearMotor.Value = rearMotorValue;

            decimal rearServoValue = ((numericUpDownRearServo.Maximum - numericUpDownRearServo.Minimum) / 2) * (decimal)(s.AxisZ) + rearServoOffset;
            if (rearServoValue < numericUpDownRearServo.Minimum)
                numericUpDownRearServo.Value = numericUpDownRearServo.Minimum;
            else if (rearServoValue > numericUpDownRearServo.Maximum)
                numericUpDownRearServo.Value = numericUpDownRearServo.Maximum;
            else
                numericUpDownRearServo.Value = rearServoValue;

            
        }

        private void checkBoxJoystick_CheckedChanged(object sender, EventArgs e)
        {
            timerJoystick.Enabled = checkBoxJoystick.Checked;
        }

        private void buttonAbort_Click(object sender, EventArgs e)
        {
            Abort();
        }

        void Abort()
        {
            timerJoystick.Enabled = false;
            checkBoxJoystick.Checked = false;
            numericUpDownThrottle.Value = numericUpDownThrottle.Minimum;
            numericUpDownLeftMotor.Value = numericUpDownLeftMotor.Minimum;
            numericUpDownRightMotor.Value = numericUpDownRightMotor.Minimum;
            numericUpDownRearMotor.Value = numericUpDownRearMotor.Minimum;
            numericUpDownRearServo.Value = 0;
            
        }
    }
}
